import rclpy
from rclpy.node import Node
from px4_msgs.msg import VehicleStatus
from geometry_msgs.msg import Twist
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node


from sls_controller.sls_missionsFactory import MissionsFactory
from sls_controller.sls_missionsSchedule import MissionsSchedule
from sls_controller.sls_meigc import RemoteService
from sls_controller.sls_uavResource import UAVResource
from sls_controller.sls_ugvResource import UGVResource
from sls_controller.sls_publicResource import PublicResource, ROSResource
from sls_controller.sls_publicDefine import *
import threading
import time
import numpy as np
import json

from libs.sls_mirrors.sls_agent import MirrorsAgent

missionTakeoffJson = {"missionsType":"MissionsTakeoff","datasets":{"takeoff_relative_altitude":6.0,"yaw":1.57}}
missionLandJson ={"missionsType":"MissionsLand","datasets":{"landing_detection_time":1,"yaw":None}}
missionQRLandJson ={"missionsType":"MissionsQRLand","datasets":None}
missionsDroneDirectPursuitCarJson = {"missionsType":"MissionsDroneDirectPursuitCar","datasets":None}
missionsSimpleJson = {"missionsType":"MissionsSimple","datasets":None}
missionsCarFollowPointJson = {"missionsType":"MissionsCarFollowPoint","datasets":{"GNSS_position": [32.1994253,119.5074,0.0],"vehicle_yaw": 0.}}
missionsCarFollowDroneJson = {"missionsType":"MissionsCarFollowDrone","datasets":None}
missionsUAVLifecycleActivateJson = {"missionsType":"MissionsUAVLifecycleActivate","datasets":[]}
missionsLifecycleDeactivateJson = {"missionsType":"MissionsLifecycleDeactivate","datasets": []}
missionsUAVFinishedFlagJson = {"missionsType":"MissionsUAVFinishedFlag","datasets": None}
missionsAddUGVTasksJson = {"missionsType":"MissionsAddUGVTasks","datasets": None}
class TimeCostRecorder:
    def __init__(self, name="算法"):
        self.name = name
        self.start_time = None
        self.enabled = True
        
    def start(self):
        """开始计时"""
        if not self.enabled:
            return
        self.start_time = time.time()
        
    def end(self):
        """结束计时并输出执行时间"""
        if not self.enabled:
            return
            
        if self.start_time is None:
            print(f"{self.name}计时器未启动")
            return
            
        end_time = time.time()
        duration = (end_time - self.start_time) * 1000  # 转换为毫秒
        print(f"{self.name}执行时间: {duration:.2f}毫秒")
        self.start_time = None
        
    def enable(self):
        """启用计时器"""
        self.enabled = True
        
    def disable(self):
        """停用计时器"""
        self.enabled = False
        self.start_time = None



class MainController(Node):
    def __init__(self):
        super().__init__('mainController_node')
        # 参数初始化
        # vehicleID
        self.declare_parameter('vehicleID', 'uav1')
        # local_host
        self.declare_parameter('local_host_mode', True)
        #uav
        #multi simulation
        self.declare_parameter('multi_simulation_mode', False)
        self.declare_parameter('reconnect_max', 2)
        #ugv
        #simulation
        self.declare_parameter('ugv_simulation_mode', False)

        self.vehicleID = self.get_parameter('vehicleID').get_parameter_value().string_value.lower()
        self.get_logger().info("vehicleID: %s" % self.vehicleID)
        if self.vehicleID.startswith("uav"):
            self.vehicleClass = VehicleClass.UAV
            self.uav_simulation_mode = self.get_parameter('multi_simulation_mode').get_parameter_value().bool_value
            self.get_logger().info("multi_simulation_mode: %s" % self.uav_simulation_mode)
            self.reconnect_attempts_max = self.get_parameter('reconnect_max').get_parameter_value().integer_value
            self.get_logger().info("reconnect_max: %s" % self.reconnect_attempts_max)
        elif self.vehicleID.startswith("ugv"):
            self.vehicleClass = VehicleClass.UGV
            self.ugv_simulation_mode = self.get_parameter('ugv_simulation_mode').get_parameter_value().bool_value
        self.local_host_mode = self.get_parameter('local_host_mode').get_parameter_value().bool_value
        self.get_logger().info("local_host_mode: %s" % self.local_host_mode)
        


        self.first_enter_flag = True
        self.main_timer_cost_recorder = TimeCostRecorder("main_timer")
        self.main_timer_cost_recorder.disable()
        self.algorithm_cost_recorder = TimeCostRecorder("algorithm_cost")
        self.algorithm_cost_recorder.disable()

        #根据需要调整执行频率
        self.main_timer_during = ParamDefault.MAIN_TIMER_DURING
        #！！！考虑异步方式执行
        #self.main_timer = self.create_timer(self.main_timer_during,self.main_timer_callback)
        #资源初始化
        ##ROS资源初始化
        self.rosResource = ROSResource(main_node = self)
        self.mirrors_agent = MirrorsAgent(local_host_mode=self.local_host_mode)
        ##车辆资源初始化
        if self.vehicleClass == VehicleClass.UAV:
            #UAV的初始化
            ##UAV的资源镜像初始化
            self.vehicleResource = UAVResource(main_node = self, idStr = self.vehicleID)
            self.publicResource = PublicResource(self.rosResource,self.mirrors_agent,uav_resource=self.vehicleResource)
        elif self.vehicleClass == VehicleClass.UGV:
            #UGV的初始化
            ##UGV的资源镜像初始化
            self.vehicleResource = UGVResource(main_node = self, idStr = self.vehicleID)
            self.publicResource = PublicResource(self.rosResource,self.mirrors_agent,ugv_resource=self.vehicleResource)
        #公共资源初始化
        self.missionsFactory = MissionsFactory(self.publicResource)      
        self.missionsSchedule = MissionsSchedule(self.missionsFactory)
        self.remoteSerivce = RemoteService(self.missionsSchedule,self.missionsFactory,self.publicResource,self.local_host_mode)
        #mirrors agent  connect
        self.mirrors_agent.connect()
        #main thread start
        threading.Thread(target=self.main_timer_callback, daemon=True).start()

    def main_timer_callback(self) -> None:
        while True:
            self.main_timer_cost_recorder.start()
            loop_start = time.time()            
            if self.first_enter_flag == True:
                self.first_enter_callback()
                self.first_enter_flag = False

            if self.vehicleClass == VehicleClass.UAV:
                if self.vehicleResource.is_nav_state_offboard_mode():
                    self.main_task()
                else:
                    #断线重连,使用指数退避策略
                    if not hasattr(self, 'reconnect_attempts'):
                        self.reconnect_attempts = 0
                    if not hasattr(self, 'last_reconnect_time'):
                        self.last_reconnect_time = 0
                        
                    current_time = time.time()
                    if current_time - self.last_reconnect_time >= min(30, 2**self.reconnect_attempts):
                        if self.reconnect_attempts < self.reconnect_attempts_max:
                            self.vehicleResource.set_nav_state_offboard_mode()
                            self.reconnect_attempts += 1
                        self.last_reconnect_time = current_time
            elif self.vehicleClass == VehicleClass.UGV:
                #逻辑
                self.main_task()


            processing_time = time.time() - loop_start
            sleep_time = max(0, self.main_timer_during - processing_time)
            time.sleep(sleep_time)
            self.main_timer_cost_recorder.end()
    

    #第一次运行要做的事
    def first_enter_callback(self):  
        #test code
        # missionTakeoffDict =  {
        #     "missionsType": "MissionsTakeoff",
        #     "datasets": {
        #         "takeoff_relative_altitude": 3,
        #         "yaw": 1.57
        #     }
        # }
        if self.vehicleClass == VehicleClass.UAV:
            #开始Offboard模式
            self.vehicleResource.set_nav_state_offboard_mode()
            #加入一些默认的任务
            missionTakeoffStr =json.dumps(missionTakeoffJson)
            missionQRLandStr =json.dumps(missionQRLandJson)
            missionLandStr =json.dumps(missionLandJson)
            missionsUAVLifecycleActivateStr = json.dumps(missionsUAVLifecycleActivateJson)
            missionsLifecycleDeactivateStr = json.dumps(missionsLifecycleDeactivateJson)
            missionsUAVFinishedFlagStr = json.dumps(missionsUAVFinishedFlagJson)
            missionsAddUGVTasksStr = json.dumps(missionsAddUGVTasksJson)
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionsAddUGVTasksStr))
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionsLifecycleDeactivateStr))
            # MissionsDroneDirectPursuitCarStr = json.dumps(missionsDroneDirectPursuitCarJson)
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionTakeoffStr))
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionQRLandStr))
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(MissionsDroneDirectPursuitCarStr))
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionLandStr))
        elif self.vehicleClass == VehicleClass.UGV:
            missionsCarFollowPointStr = json.dumps(missionsCarFollowPointJson)
            missionsCarFollowDroneStr =json.dumps(missionsCarFollowDroneJson)
            missionsSimpleStr = json.dumps(missionsSimpleJson)
            # self.missionsSchedule.append_mission(self.missionsFactory.create_missions_from_json(missionsCarFollowDroneStr))
        # 一般不要直接开始任务，建议通过上位机开始任务
        # self.missionsSchedule.startMissions()
        pass




    def main_task(self):
        self.algorithm_cost_recorder.start()
        #1 algorithm using missionsSchedule and publicResource
        #2 remote agent service
        # if not self.meigc.is_connected():
            # self.meigc.reconnect()
        #3 missionsSchedule
        self.missionsSchedule.schedule_task()
        #self.complexMissionsSchedule.schedule_task()
        #4
        #print(self.publicResource.publicSource.vehicle_status.takeoff_time)
        #print(self.vehicle_status.timestamp-self.vehicle_status.takeoff_time)
        self.algorithm_cost_recorder.end()

def main(args=None) -> None:
    print('Starting offboard control node...')
    rclpy.init(args=args)
    mainController = MainController()
    #利用多线程来避免回调函数的阻塞（定时回调，发布和订阅回调）
    #executor = MultiThreadedExecutor()
    #executor.add_node(mainController)
    #executor.spin()
    rclpy.spin(mainController)
    mainController.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        print(e)
